#include "apps/BehaviorCtrlApp.h"

namespace behavior_controller
{
BehaviorController::BehaviorController(int argc, char **argv)
{
    //开启ros node
    rclcpp::init(argc, argv);
    node_handle = rclcpp::Node::make_shared("behavior_controller");
    // tf
    tf_buffer = std::make_shared<tf2_ros::Buffer>(node_handle->get_clock());
    tf_buffer->setUsingDedicatedThread(true);
    tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);
    ipa_room_plan = std::make_shared<IpaRoomPlanning>(node_handle, tf_buffer);
    //para
    ParamLoadModule param_load;
    GetDebugPara(param_load);
    // std::thread topic_thread(std::bind(&BehaviorController::topicFun,this));
    std::thread transform_thread(std::bind(&BehaviorController::transformFun, this));
    // module get inter constraints different trajectories !
    // 模块得到内部约束不同的轨迹!
    TimeModule time_module;
    time_module.InstallFunctors();
    // 监控
    monitor_module = std::make_shared<MonitorModule>(node_handle);
    monitor_module->InstallFunctors();
    // 控制模块
    ctrl_module = std::make_shared<CtrlModule>(node_handle, &lock, tf_buffer, ipa_room_plan);
    ctrl_module->InstallFunctors();
    ///////std::thread monitor_thread(std::bind(&BehaviorController::thread_monitor_fun,this));
    //-------------------------时间频率发布-------------------------//
    ctrl_module->Work();
    ///////monitor_thread.join();

    //-------------------------坐标系转换测�?-------------------------//
    /*PoseF robot_pose0(2.0f, 1.0f, .0f);
    PoseF view_pose(1.0f, 3.0f, .0f);
    LogPub::Info("behavior_controller", "[BC] 将机器人(2,1)坐标系下�?1,3）位�?的传感器点，�?为世界坐标系下的坐标�?");
    auto result = robot_pose0.ConvertOnCur(view_pose);
    LogPub::Info("behavior_controller", "[BC] result: x�?", result.GetPoint().x(), "y:", result.GetPoint().y());

    PoseF robot_pose1(2.0f, 1.0f, .0f);
    PoseF view_pose1(3.0f, 4.0f, .0f);
    result = robot_pose1.ConvertOriginCur(view_pose1);
    LogPub::Info("behavior_controller", "[BC] 将世界坐标系下的�?3,4）点�?为机器人(2,1)坐标系下的点坐标�?");
    LogPub::Info("behavior_controller", "[BC] result: x�?", result.GetPoint().x(), "y:", result.GetPoint().y());
    */
    //-------------------------点云坐标系转换测�?-------------------------//
    /*
    PointCloud<double> world_point_cloud;
    world_point_cloud.emplace_back(PointF(3, 3));
    world_point_cloud.emplace_back(PointF(4, 4));
    world_point_cloud.emplace_back(PointF(5, 5));
    world_point_cloud.emplace_back(PointF(6, 6));
    world_point_cloud.emplace_back(PointF(7, 7));
    PoseF robot_pose0(2.0f, 1.0f, .0f);
    LogPub::Info("behavior_controller", "[BC] 将世界坐标系下的点云�?为机器人(2,1)坐标系下的点云数�?�?");
    PointCloud<double> local_point_cloud = robot_pose0.ConvertToLocalCloud(world_point_cloud);
    int index = 0;
    for (auto pt : local_point_cloud)
    {
        index++;
        LogPub::Info("behavior_controller", "[BC] 点[", index, "]:x(", pt.GetPoint().x(), ") y(", pt.GetPoint().y(), ")");
    }
    index = 0;
    LogPub::Info("behavior_controller", "[BC] 将机器人(2,1)坐标系下的点云转为世界坐标系下的点云数据�?");
    world_point_cloud.clear();
    world_point_cloud = robot_pose0.ConvertToWorldCloud(local_point_cloud);
    for (auto pt : world_point_cloud)
    {
        index++;
        LogPub::Info("behavior_controller", "[BC] 点[", index, "]:x(", pt.GetPoint().x(), ") y(", pt.GetPoint().y(), ")");
    }*/
    //-------------------------�?廓测�?-------------------------//
    /*
    ContourAreas contour_areas;
    contour_areas.Load(param_load.contour_yaml_);
    Contour robot_shape_ = contour_areas.robot_shape();
    std::string result1 = (robot_shape_.IsPointInShape(PointF(0.2f, 0.2f))) ? "true" : "false";
    std::string result2 = (robot_shape_.IsPointInShape(PointF(0.4f, 0.4f))) ? "true" : "false";
    LogPub::Info("behavior_controller", "[BC] (0.2, 0.2)点在�?廓内�?", result1);
    LogPub::Info("behavior_controller", "[BC] (0.4, 0.4)点在�?廓内�?", result2);*/
    //-------------------------直线测试-------------------------//
    /*PointCloud<double> world_point_cloud;
    //for (double i = 0; i < 50; i = i + 5)
    //{
    //    world_point_cloud.emplace_back(PointF((double)i, (double)(2*i+1)));
    // }
    world_point_cloud.emplace_back(PointF(-0.510784, -0.294901));
    world_point_cloud.emplace_back(PointF(-0.507937, -0.30322));
    world_point_cloud.emplace_back(PointF(-0.487117, -0.300513));
    world_point_cloud.emplace_back(PointF(-0.466602, -0.297338));
    world_point_cloud.emplace_back(PointF(-0.45053, -0.296423));
    world_point_cloud.emplace_back(PointF(-0.451857, -0.306831));
    world_point_cloud.emplace_back(PointF(-0.426084, -0.298502));
    world_point_cloud.emplace_back(PointF(-0.416767, -0.301131));
    world_point_cloud.emplace_back(PointF(-0.413893, -0.308341));
    world_point_cloud.emplace_back(PointF(-0.37793, -0.290215));
    //world_point_cloud.emplace_back(PointF(0, 0));
    //world_point_cloud.emplace_back(PointF(0, 0));
    //world_point_cloud.emplace_back(PointF(0, 0));
    //world_point_cloud.emplace_back(PointF(0, 0));

    double line_k, line_b,R,x_mean,y_mean;
    Funcs::LineFit(world_point_cloud, line_k, line_b);
    std::cout << "Line: line_k = " << line_k << " line_b= " << line_b << std::endl;
    Funcs::LineFit(world_point_cloud, line_k, line_b,R,x_mean,y_mean);
    std::cout << "Line: line_k = " << line_k << " line_b= " << line_b << " r= " << R <<std::endl;*/
    //-------------------------激光数�?接收、激光数�?�?点云数据测试-------------------------//
    //SubScan::SubScanInfo(node_handle);
    //-------------------------速度发布测试-------------------------//
    /*VelPubModule vel_pub_mod1(node_handle);
    while (1)
    {
        vel_pub_mod1.VelPub(0.3f, 0.0f);
    }*/
}

void BehaviorController::GetDebugPara(ParamLoadModule &param_load)
{
    param_load.LoadLogYaml();
    contour_areas.Load(param_load.contour_yaml_);
    ////////////////////////////////////////////////////////////////////////////////
    GetPara<int>(param_load.parameters_yaml_, "behavior", "sim_mode", SIM_MODE);
    GetPara<int>(param_load.parameters_yaml_, "behavior", "follow_wall", FOLLOW_WALL);

    if (SIM_MODE == 1)
    {
        std::cout << "SIM_MODE !" << std::endl;
        return;
    }
    GetPara<int>(param_load.parameters_yaml_, "behavior", "use_local", USE_ROBOT_FRAME);
    GetPara<double>(param_load.parameters_yaml_, "behavior", "robot_radius", ROBOT_RADIUS);
    RADIUS_SQUA = ROBOT_RADIUS * ROBOT_RADIUS;
    ROBOT_DIAMETER = 2 * ROBOT_RADIUS;
    DIAMETER_SQUA = ROBOT_DIAMETER * ROBOT_DIAMETER;
    GetPara<double>(param_load.parameters_yaml_, "behavior", "cliff_maxdist", CLIFF_MAXDIST);
    GetPara<double>(param_load.parameters_yaml_, "behavior", "cliff_obsdist", CLIFF_OBSDIST);
    GetPara<double>(param_load.parameters_yaml_, "behavior", "margin_right", MARGIN_RIGHT);
    GetPara<double>(param_load.parameters_yaml_, "behavior", "margin_left", MARGIN_LEFT);
    GetPara<double>(param_load.parameters_yaml_, "behavior", "margin_turndist", MARGIN_TURNDIST);

    GetPara<double>(param_load.parameters_yaml_, "behavior", "margin_maxdist", MARGIN_MAXDIST);
    GetPara<double>(param_load.parameters_yaml_, "behavior", "margin_obsdist", MARGIN_OBSDIST);
    GetPara<double>(param_load.parameters_yaml_, "behavior", "margin_p", MARGIN_P);
    GetPara<double>(param_load.parameters_yaml_, "behavior", "margin_i", MARGIN_I);
    GetPara<double>(param_load.parameters_yaml_, "behavior", "margin_d", MARGIN_D);
    /////////////////////////
    GetPara<double>(param_load.parameters_yaml_, "behavior", "linear1", t_linear1);
    GetPara<double>(param_load.parameters_yaml_, "behavior", "linear2", t_linear2);
    GetPara<double>(param_load.parameters_yaml_, "behavior", "linear3", t_linear3);
    GetPara<double>(param_load.parameters_yaml_, "behavior", "angular1", t_angular1);
    GetPara<double>(param_load.parameters_yaml_, "behavior", "angular2", t_angular2);
    GetPara<double>(param_load.parameters_yaml_, "behavior", "angular3", t_angular3);
    ///////////////////////////////////////////////////////////
    std::vector<double> pose;
    GetPara<std::vector<double >>(param_load.parameters_yaml_, "pose", "lidar_pose", pose);
    lidar_pose = PoseF(pose[0], pose[1], pose[2]);
    GetPara<std::vector<double >>(param_load.parameters_yaml_, "pose", "left_fron_cliff", pose);
    left_fron_cliff = PoseF(pose[0], pose[1], pose[2]);
    GetPara<std::vector<double >>(param_load.parameters_yaml_, "pose", "left_mid_cliff", pose);
    left_mid_cliff = PoseF(pose[0], pose[1], pose[2]);
    GetPara<std::vector<double >>(param_load.parameters_yaml_, "pose", "left_back_cliff", pose);
    left_back_cliff = PoseF(pose[0], pose[1], pose[2]);
    GetPara<std::vector<double >>(param_load.parameters_yaml_, "pose", "righ_fron_cliff", pose);
    righ_fron_cliff = PoseF(pose[0], pose[1], pose[2]);
    GetPara<std::vector<double >>(param_load.parameters_yaml_, "pose", "righ_mid_cliff", pose);
    righ_mid_cliff = PoseF(pose[0], pose[1], pose[2]);
    GetPara<std::vector<double >>(param_load.parameters_yaml_, "pose", "righ_back_cliff", pose);
    righ_back_cliff = PoseF(pose[0], pose[1], pose[2]);
    GetPara<std::vector<double >>(param_load.parameters_yaml_, "pose", "margin_pose", pose);
    margin_pose = PoseF(pose[0], pose[1], pose[2]);
    GetPara<std::vector<double >>(param_load.parameters_yaml_, "pose", "camera_pose", pose);
    camera_pose = PoseF(pose[0], pose[1], pose[2]);
    //////////////////////////////////////////////////////////////////////
    ipa_room_plan->getParem().robot_radius = ROBOT_RADIUS;
    GetPara<double>(param_load.parameters_yaml_, "plan", "coverage_radius",
                    ipa_room_plan->getParem().coverage_radius);
    GetPara<int>(param_load.parameters_yaml_, "plan", "planning_mode",
            ipa_room_plan->getParem().planning_mode);
    ////////////////////////////////////////////////////////////////////////
    std::cout << "robot_radius= " << ROBOT_RADIUS << std::endl;
    std::cout << "laser_pose= " << lidar_pose.x() << " " << lidar_pose.y()
              << " " << lidar_pose.angle() << std::endl;
    std::cout << "robot_radius= " << ipa_room_plan->getParem().robot_radius << std::endl;
    std::cout << "coverage_radius= " << ipa_room_plan->getParem().coverage_radius << std::endl;
}

void BehaviorController::thread_monitor_fun()
{
    monitor_module->Work();
}

void BehaviorController::topicFun()
{
    /*  rclcpp::Rate r(100);
     SubScan::SubScanInfo(node_handle);
     SubBattery::SubBatteryInfo(node_handle);
     SubBumper::SubBumperInfo(node_handle);
     SubCliffSensor::SubCliffSensorInfo(node_handle);
     SubImu::SubImuInfo(node_handle);
     SubMarginSensor::SubMarginSensorInfo(node_handle);
     SubMotor::SuMotorInfo(node_handle);
     SubObjectPoseArray::SubObjectPoseArrayInfo(node_handle);
     SubOdom::SubOdomInfo(node_handle);
     SubOffGroundSwitch::SubOffGroundSwitchInfo(node_handle);
     SubTwist::SubTwistInfo(node_handle);
     while(rclcpp::ok())
     {
         rclcpp::spin_some(node_handle);
         r.sleep();
     } */
}

void BehaviorController::transformFun()
{
    rclcpp::Rate r(100);
    tf2::Quaternion quat;
    geometry_msgs::msg::PoseStamped global_pose, robot_pose_msg;
    std::string global_frame = "map";
    std::string robot_frame = "base_link";
    global_pose.header.frame_id = global_frame;
    robot_pose_msg.header.frame_id = robot_frame;
    double roll, pitch, yaw;
    double transform_timeout = 5;
    bool flag_imu_pose = false;
    // 雷达
    SubScan::SubScanInfo(node_handle);
    // 电池
    SubBattery::SubBatteryInfo(node_handle);
    // 保险杠
    SubBumper::SubBumperInfo(node_handle);
    // 探地传感器
    SubCliffSensor::SubCliffSensorInfo(node_handle);
    // imu
    SubImu::SubImuInfo(node_handle);
    // 沿边传感器
    SubMarginSensor::SubMarginSensorInfo(node_handle);
    // 电机
    SubMotor::SuMotorInfo(node_handle);
    SubObjectPoseArray::SubObjectPoseArrayInfo(node_handle);
    SubOdom::SubOdomInfo(node_handle);
    SubOffGroundSwitch::SubOffGroundSwitchInfo(node_handle);
    SubTwist::SubTwistInfo(node_handle);
    while (rclcpp::ok())
    {
        try
        {
            tf2::Quaternion quaternion = tf2::Transform::getIdentity().getRotation();
            global_pose.pose.orientation = tf2::toMsg(quaternion);
            robot_pose_msg.pose.orientation = tf2::toMsg(quaternion);
            robot_pose_msg.header.stamp = node_handle->now();
            global_pose = tf_buffer->transform(robot_pose_msg, "map", tf2::durationFromSec(transform_timeout));
            tf2::Quaternion quat = tf2::Quaternion(
                    global_pose.pose.orientation.x, global_pose.pose.orientation.y,
                    global_pose.pose.orientation.z, global_pose.pose.orientation.w);
            tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);
            DataSet::robot_pose = PoseF(global_pose.pose.position.x, global_pose.pose.position.y, yaw);
//            std::cout << "robot_pose x: " << DataSet::robot_pose.x()
//                      << " y: " << DataSet::robot_pose.y()
//                      << " angle: " << DataSet::robot_pose.angle()
//                      << std::endl;
        }
        catch (tf2::LookupException &ex)
        {
            RCLCPP_ERROR(node_handle->get_logger(),
                         "No Transform available Error looking up robot pose: %s\n", ex.what());
        }
        catch (tf2::ConnectivityException &ex)
        {
            RCLCPP_ERROR(node_handle->get_logger(),
                         "Connectivity Error looking up robot pose: %s\n", ex.what());
        }
        catch (tf2::ExtrapolationException &ex)
        {
            RCLCPP_ERROR(node_handle->get_logger(),
                         "Extrapolation Error looking up robot pose: %s\n", ex.what());
        }
        catch (tf2::TimeoutException &ex)
        {
            RCLCPP_ERROR(node_handle->get_logger(),
                         "Transform timeout with tolerance: %.4f", transform_timeout);
        }
        catch (tf2::TransformException &ex)
        {
            RCLCPP_ERROR(node_handle->get_logger(),
                         "Failed to transform from %s to %s", global_frame.c_str(), robot_frame.c_str());
        }
        if (!flag_imu_pose)
        {
            if (DataSet::imu_data.GetRealTime() > 0)
            {
                try
                {
                    std::cout << "DataSet::imu_data.imu_frame " << DataSet::imu_data.GetFrame() << std::endl;
                    geometry_msgs::msg::TransformStamped trans =
                            tf_buffer->lookupTransform(
                                    robot_frame,
                                    DataSet::imu_data.GetFrame(),
                                    tf2::TimePointZero);
                    ImuData::ImuInRobot imu_in_pose;
                    imu_in_pose.position = Eigen::Vector3d(trans.transform.translation.x,
                                                           trans.transform.translation.y,
                                                           trans.transform.translation.z);
                    imu_in_pose.quaternion = Eigen::Quaterniond(trans.transform.rotation.w,
                                                                trans.transform.rotation.x,
                                                                trans.transform.rotation.y,
                                                                trans.transform.rotation.z);
                    DataSet::imu_data.set_imu_pose(imu_in_pose);
                    flag_imu_pose = true;
                }
                catch (tf2::TransformException &ex)
                {
                    RCLCPP_ERROR(node_handle->get_logger(), "Failed to transform from %s to %s",
                                 robot_frame.c_str(), DataSet::imu_data.GetFrame().c_str());
                }
            }
        }
        rclcpp::spin_some(node_handle);
        r.sleep();
    }
}

template<typename T>
bool GetPara(YAML::Node yaml_, std::string domain_name, std::string param_name, T &value)
{
    if (yaml_[domain_name].IsDefined())
    {
        if (yaml_[domain_name][param_name].IsDefined())
        {
            value = yaml_[domain_name][param_name].as<T>();
            return true;
        }
        else
        {
            std::cout << "error: no " << param_name << std::endl;
            return false;
        }
    }
    else
    {
        std::cout << "error: no " << domain_name << std::endl;
        return false;
    }
}

} // namespace behavior_controller

int main(int argc, char **argv)
{
    behavior_controller::BehaviorController controller(argc, argv);
    return 0;
}
